% 求解优化问题(有障碍物的最优路径)
% x = [x2;...;x_{H-1};y2;...;y2;y_{2H-1};Δt;t'] α
% 改变(x0,y0),(xf,yf),H,a,b,c,d可以求不同情况下的最优路径
% 使用了作者写的irma.m函数

clear
close all

%initial condition
x0 =  0; y0 =  0;
xf = 10; yf = 10;
H = 10;  %要求H>=6
V = 1;
u_max = 0.2;
w = 1.5;
delta = 1e-4;
% a = [1.5,1.5,2.5];  %a,b,c,d为椭圆障碍物参数，椭圆的方程为(xi-cj)^2/aj^2-(yi-dj)^2/bj^2=1
% b = [2,1.5,1.5];
% c = [6,2,6];
% d = [6,4,2];
a = [5];  %a,b,c,d为椭圆障碍物参数，椭圆的方程为(xi-cj)^2/aj^2-(yi-dj)^2/bj^2=1
b = [5];
c = [5];
d = [5];
m = size(a,2);  %椭圆障碍物个数
l_x = [-2*ones(H-2,1);  %x的下界
    0*ones(H-2,1);      %y的下界
    0;                  %Δt的下界
    0];                 %t'的下界
u_x = [10.2*ones(H-2,1);%x的上界
    12*ones(H-2,1);     %y的上界
    20/(H-1);           %Δt的上界
    (20/(H-1))^2];      %t'的上界

%define objective matrix
Q0 = zeros(2*H-1,2*H-1);  %minimize( (H-1)*Δt )
Q0(2*H-1,2*H-3) = (H-1)/2;
Q0(2*H-3,2*H-1) = (H-1)/2;

%define equality constraints
Eqcon(1).Q = zeros(2*H-1,2*H-1); %α^2=1
Eqcon(1).Q(2*H-1,2*H-1) = 1;
Eqcon(1).c = -1;

Eqcon(2).Q = zeros(2*H-1,2*H-1); %t'=(Δt)^2
Eqcon(2).Q(2*H-3,2*H-3) = -1;
Eqcon(2).Q(2*H-1,2*H-2) = 0.5;
Eqcon(2).Q(2*H-2,2*H-1) = 0.5;
Eqcon(2).c = 0;

Eqcon(3).Q = zeros(2*H-1,2*H-1); %x2^2+y2^2- V^2*(Δt)^2=0
Eqcon(3).Q(1,1) = 1;
Eqcon(3).Q(H-1,H-1) = 1;
Eqcon(3).Q(2*H-1,1) = -x0;
Eqcon(3).Q(1,2*H-1) = -x0;
Eqcon(3).Q(2*H-1,H-1) = -y0;
Eqcon(3).Q(H-1,2*H-1) = -y0;
Eqcon(3).Q(2*H-3,2*H-3) = -V^2;
Eqcon(3).c = x0^2+y0^2;

%(x_{h+1}-x_h)^2 + (y_{h+1}-y_h)^2 - V^2*(Δt)^2 = 0, h=2,...,H-2
for i = 1:H-3
    Eqcon(3+i).Q = zeros(2*H-1,2*H-1);
    Eqcon(3+i).Q(i,i) = 1;
    Eqcon(3+i).Q(i,i+1) = -1;
    Eqcon(3+i).Q(i+1,i) = -1;
    Eqcon(3+i).Q(i+1,i+1) = 1;
    Eqcon(3+i).Q(H-2+i,H-2+i) = 1;
    Eqcon(3+i).Q(H-2+i,H-2+i+1) = -1;
    Eqcon(3+i).Q(H-2+i+1,H-2+i) = -1;
    Eqcon(3+i).Q(H-2+i+1,H-2+i+1) = 1;
    Eqcon(3+i).Q(2*H-3,2*H-3) = -V^2;
    Eqcon(3+i).c = 0;
end

%x_{H-1}^2 + y_{H-1}^2 - 20*x_{H-1} - 20*y_{H-1} - V^2*(Δt)^2=0
Eqcon(H+1).Q = zeros(2*H-1,2*H-1);
Eqcon(H+1).Q(H-2,H-2) = 1;
Eqcon(H+1).Q(2*H-4,2*H-4) = 1;
Eqcon(H+1).Q(2*H-1,H-2) = -xf;
Eqcon(H+1).Q(H-2,2*H-1) = -xf;
Eqcon(H+1).Q(2*H-1,2*H-4) = -yf;
Eqcon(H+1).Q(2*H-4,2*H-1) = -yf;
Eqcon(H+1).Q(2*H-3,2*H-3) = -V^2;
Eqcon(H+1).c = xf^2+yf^2;

%define inequality constraints
%l_x<=x<=u_x
for i = 1:2*H-2  %x2~x_{H-1}的取值范围
    Incon(i).Q = zeros(2*H-1,2*H-1); %x(i)-u_x(i)<=0
    Incon(i).Q(2*H-1,i) = 0.5;
    Incon(i).Q(i,2*H-1) = 0.5;
    Incon(i).c = -u_x(i);
end
for i = 1:2*H-2  %x2~x_{H-1}的取值范围
    Incon(2*H-2+i).Q = zeros(2*H-1,2*H-1); %-x(i)+l_x(i)<=0
    Incon(2*H-2+i).Q(2*H-1,i) = -0.5;
    Incon(2*H-2+i).Q(i,2*H-1) = -0.5;
    Incon(2*H-2+i).c = l_x(i);
end

%x3^2+4*x2^2 - 4*x2*x3 + x3^2+4*x2^2 - 4*x2*x3 - V^2*u_max^2*(t')^2 = 0
Incon(4*H-3).Q = zeros(2*H-1,2*H-1); 
Incon(4*H-3).Q(1,1) = 4;
Incon(4*H-3).Q(1,2) = -2;
Incon(4*H-3).Q(2,1) = -2;
Incon(4*H-3).Q(2,2) = 1;
Incon(4*H-3).Q(H-1,H-1) = 4;
Incon(4*H-3).Q(H-1,H) = -2;
Incon(4*H-3).Q(H,H-1) = -2;
Incon(4*H-3).Q(H,H) = 1;
Incon(4*H-3).Q(2*H-1,1) = -2*x0;
Incon(4*H-3).Q(1,2*H-1) = -2*x0;
Incon(4*H-3).Q(2*H-1,2) = x0;
Incon(4*H-3).Q(2,2*H-1) = x0;
Incon(4*H-3).Q(2*H-1,H-1) = -2*y0;
Incon(4*H-3).Q(H-1,2*H-1) = -2*y0;
Incon(4*H-3).Q(2*H-1,H) = y0;
Incon(4*H-3).Q(H,2*H-1) = y0;
Incon(4*H-3).Q(1,1) = 4;

Incon(4*H-3).Q(2*H-2,2*H-2) = -V^2*u_max^2;
Incon(4*H-3).c = x0^2+y0^2;

for i = 1:H-4
    Incon(4*H-3+i).Q = zeros(2*H-1,2*H-1);
    Incon(4*H-3+i).Q(i,i) = 1;
    Incon(4*H-3+i).Q(i,i+1) = -2;
    Incon(4*H-3+i).Q(i,i+2) = 1;
    Incon(4*H-3+i).Q(i+1,i) = -2;
    Incon(4*H-3+i).Q(i+1,i+1) = 4;
    Incon(4*H-3+i).Q(i+1,i+2) = -2;
    Incon(4*H-3+i).Q(i+2,i) = 1;
    Incon(4*H-3+i).Q(i+2,i+1) = -2;
    Incon(4*H-3+i).Q(i+2,i+2) = 1;
    Incon(4*H-3+i).Q(H-2+i,H-2+i) = 1;
    Incon(4*H-3+i).Q(H-2+i,H-2+i+1) = -2;
    Incon(4*H-3+i).Q(H-2+i,H-2+i+2) = 1;
    Incon(4*H-3+i).Q(H-2+i+1,H-2+i) = -2;
    Incon(4*H-3+i).Q(H-2+i+1,H-2+i+1) = 4;
    Incon(4*H-3+i).Q(H-2+i+1,H-2+i+2) = -2;
    Incon(4*H-3+i).Q(H-2+i+2,H-2+i) = 1;
    Incon(4*H-3+i).Q(H-2+i+2,H-2+i+1) = -2;
    Incon(4*H-3+i).Q(H-2+i+2,H-2+i+2) = 1;
    Incon(4*H-3+i).Q(2*H-2,2*H-2) = -V^2*u_max^2;
    Incon(4*H-3+i).c = 0;
end

%x_{H-2}^2+4*x_{H-1}^2+20*x_{H-2}-40*x_{H-1}-4*x_{H-2}*x_{H-1}+
%y_{H-2}^2+4*y_{H-1}^2+20*y_{H-2}-40*y_{H-1}-4*y_{H-2}*y_{H-1}
%-V^2*u_max^2*(t')^2+200 = 0
Incon(5*H-6).Q = zeros(2*H-1,2*H-1); 
Incon(5*H-6).Q(H-3,H-3) = 1;
Incon(5*H-6).Q(H-3,H-2) = -2;
Incon(5*H-6).Q(H-2,H-3) = -2;
Incon(5*H-6).Q(H-2,H-2) = 4;
Incon(5*H-6).Q(2*H-1,H-3) = xf;
Incon(5*H-6).Q(H-3,2*H-1) = xf;
Incon(5*H-6).Q(2*H-1,H-2) = -2*xf;
Incon(5*H-6).Q(H-2,2*H-1) = -2*xf;
Incon(5*H-6).Q(2*H-5,2*H-5) = 1;
Incon(5*H-6).Q(2*H-5,2*H-4) = -2;
Incon(5*H-6).Q(2*H-4,2*H-5) = -2;
Incon(5*H-6).Q(2*H-4,2*H-4) = 4;
Incon(5*H-6).Q(2*H-1,2*H-5) = yf;
Incon(5*H-6).Q(2*H-5,2*H-1) = yf;
Incon(5*H-6).Q(2*H-1,2*H-4) = -2*yf;
Incon(5*H-6).Q(2*H-4,2*H-1) = -2*yf;
Incon(5*H-6).Q(2*H-2,2*H-2) = -V^2*u_max^2;
Incon(5*H-6).c = xf^2+yf^2;

%1-(xi-cj)^2/aj^2-(yi-dj)^2/bj^2 <= 0
for j =1:m
    for i = 1:H-2
        Incon(5*H-6+(j-1)*(H-2)+i).Q = zeros(2*H-1,2*H-1);
        Incon(5*H-6+(j-1)*(H-2)+i).Q(i,i) = -1/a(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).Q(H-2+i,H-2+i) = -1/b(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).Q(2*H-1,i) = c(j)/a(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).Q(2*H-1,H-2+i) = d(j)/b(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).Q(i,2*H-1) = c(j)/a(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).Q(H-2+i,2*H-1) = d(j)/b(j)^2;
        Incon(5*H-6+(j-1)*(H-2)+i).c = 1-c(j)^2/a(j)^2-d(j)^2/b(j)^2;
    end
end

output = irma(Q0,Incon,Eqcon);   %call irma function

%draw the path
x = output.x;
plot([x0;x(1:H-2);xf],[y0;x(H-1:2*H-4);yf],'--bo','LineWidth',2)
xlabel('\fontname{Times new roman}\it x')
ylabel('\fontname{Times new roman}\it y')
title(['\fontname{Times new roman}',num2str(H),'\fontname{宋体}个节点',...
    '\fontname{Times new roman}',num2str(m),...
    '\fontname{宋体}个障碍，\fontname{Times new roman}IRM',...
    '\fontname{宋体}法求解的路径'])
axis equal
hold on
%draw the obstacles
theta = 0:0.01:2*pi;
for i = 1:m
    xp = c(i) + a(i) * cos(theta);
    yp = d(i) + b(i) * sin(theta);
    plot(xp, yp, '.r');
end